#include "circle_detection/CircleDetection.h"
#define Display 1
#define Display_forDebug 0
#define REALRACE 1

#define TWOCIRCLES 2
#define ONECIRCLESONEELLIPSE 1
#define TWOELLIPSE 0

#define IMAGE_WIDTH 640
#define IMAGE_HEIGHT 480
#define circle_r 600
#define uav_size 450

//red circles
#define R_MIN 120
#define R_MAX 255
#define G_MIN 0
#define G_MAX 70
#define B_MIN 10
#define B_MAX 90

//yellow circles
#define R_MIN11 180
#define R_MAX11 240
#define G_MIN11 170
#define G_MAX11 230
#define B_MIN11 30
#define B_MAX11 120
#define farawayTH 3500

const double baseline = 95.0;
const double fx = 320.0;
const double cx = 320.0;
const double fy = 320.0;
const double cy = 240.0;

using namespace std;
using namespace sensor_msgs;
using namespace message_filters;
using namespace cv;
using namespace Eigen;


namespace circle_detection{
CircleDetection::CircleDetection(ros::NodeHandle nh):n(nh){
        init();
}
CircleDetection::~CircleDetection(){};

#if REALRACE
void CircleDetection::init(){
    camera_K_<< 320, 0, 320, 0, 320, 240, 0, 0, 1;
    camera_K_inv_ = camera_K_.inverse();
    ROS_INFO_STREAM("camera parameters\n" << camera_K_inv_);
    initCamTopics(n);
    ROS_INFO("Circle Detection initialized!!");
    pos_sub_ =
        n.subscribe<geometry_msgs::PoseStamped>("/airsim_node/drone_1/debug/pose_gt",1, &CircleDetection::posCallback,this);
    // sub_depthimg = n.subscribe(depth_topic, 1, &CircleDetection::imgdepth_callback, this, ros::TransportHints().tcpNoDelay());
    pub_recg = n.advertise<custom_msgs::CircleRecog>("/Circle/circle_detection", 1);
    image_transport::ImageTransport it(n);
    pic_pub =  it.advertise("/image/image_depth", 1);
}
#else
void CircleDetection::init(){
    ros::param::get("~d435i_depth_topic", depth_topic);
    camera_K_<< 606.735595703125, 0.0, 329.1787414550781, 0.0, 606.5174560546875, 241.1900177001953, 0.0, 0.0, 1.0;
    camera_K_inv_ = camera_K_.inverse();
    
    ROS_INFO_STREAM("camera parameters\n" << camera_K_inv_);
    ROS_INFO("Circle Detection initialized!!");

    sub_depthimg = n.subscribe(depth_topic, 1, &CircleDetection::imgdepth_callback, this, ros::TransportHints().tcpNoDelay());
    pub_recg = n.advertise<custom_msgs::CircleRecog>("/d435/depth_recogization", 1);
}
#endif

#if REALRACE
    void CircleDetection::imageCallback(const sensor_msgs::ImageConstPtr &left_image,const sensor_msgs::ImageConstPtr &right_image)
    {
        ros::Time t0, t1;
        double get_time, remove_time, int_time, equ_time, detect_time, draw_time, circle_time;
        t0 = ros::Time::now();
        //cout << "11111111111111111111" << endl;
        left_buf = cv_bridge::toCvShare(left_image,"bgr8")->image;//将ROS中的 left_image 消息转换为OpenCV中的图像格式
        right_buf = cv_bridge::toCvShare(right_image,"bgr8")->image;
        cv::imwrite("/home/suncc/sim23/src/picture/left.png", left_buf);
        //cv::imwrite("/home/sunfy/sim_code2023_circle/src/picture/right.png", right_buf);
        disparity = computesgbm(left_image,right_image);//得到视差图，方法需要优化
        cv::imwrite("/home/suncc/sim23/src/picture/disparity.png", disparity);
        depth_buf = getDepthImageFromSence(disparity);//得到深度图
        cv::imwrite("/home/suncc/sim23/src/picture/depth.png", depth_buf);

        if (left_buf.empty()||right_buf.empty()){
            printf("No rgb Image"); 
            return;
        }
        sensor_msgs::ImagePtr msg = cv_bridge::CvImage(std_msgs::Header(), "uint16_t", depth_buf).toImageMsg();
        pic_pub.publish(msg);
        // while (nh.ok()) {
        //     pub.publish(msg);
        //     ros::spinOnce();
        //     loop_rate.sleep();
        // }

        //rgb图像二值滤波//
        vector<Vec3f> circles_l,circles_r;
        cv::Mat dst_l,dst_r;
        cv::Mat hsv_l,hsv_r;
        cv::Mat binary_left,binary_right;
        cv::Mat edge_left,edge_right;
        cvtColor(left_buf, hsv_l, COLOR_BGR2HSV);
        cvtColor(right_buf, hsv_r, COLOR_BGR2HSV);
        dst_l = cv::Mat::zeros(IMAGE_HEIGHT, IMAGE_WIDTH, CV_8UC1);
        dst_r = cv::Mat::zeros(IMAGE_HEIGHT, IMAGE_WIDTH, CV_8UC1);//8位无符号整形
        inRange(left_buf, Scalar(B_MIN, G_MIN, R_MIN), Scalar(B_MAX, G_MAX, R_MAX), dst_l);//筛选红色圈为白色，其他为黑色
        inRange(right_buf, Scalar(B_MIN, G_MIN, R_MIN), Scalar(B_MAX, G_MAX, R_MAX), dst_r);//将输入图像中的像素值在指定的颜色范围内进行筛选，并将符合条件的像素值存储在dst_r中
        RemoveFarAwayPoints(depth_buf, dst_l);//剔除深度值太大(离得太远的点)
        RemoveFarAwayPoints(depth_buf, dst_r);//剔除深度值太大(离得太远的点)
        binary_left = filterImg(dst_l);
        binary_right = filterImg(dst_r);
        //cv::imwrite("/home/sunfy/sim_code2023/src/picture/binary_a.png", binary_left);
        //cv::imwrite("/home/sunfy/sim_code2023/src/picture/binary_b.png", binary_right);

        /*采用EDcircle识别圆圈*/
        EDCircles testEDCircles = EDCircles(binary_left); //detect circles and ellipses
        detect_time = (ros::Time::now()-t0).toSec();
        t0 = ros::Time::now();

        int noCircles = testEDCircles.getCirclesNo();
        cv::Mat circleImg = testEDCircles.drawResult(true, ImageStyle::BOTH);
        if (Display) {
            // cout << depth_buf.at<uint16_t>(depth_buf.rows/2, depth_buf.cols/2);
            // cv::namedWindow("ColorImage", cv::WINDOW_NORMAL);
            // cv::moveWindow("ColorImage", 0, 0);
            // cv::resizeWindow("ColorImage", 640, 480);
            // cv::imshow("ColorImage", left_buf);
            
            // cv::waitKey(1);

            cv::namedWindow("DepthImageAfterEqualize", cv::WINDOW_NORMAL);
            cv::moveWindow("DepthImageAfterEqualize", 0, 480);
            cv::resizeWindow("DepthImageAfterEqualize", 640, 480);
            cv::imshow("DepthImageAfterEqualize", circleImg);
            cv::waitKey(1);
        }
        draw_time = (ros::Time::now()-t0).toSec();
        t0 = ros::Time::now();

        if (testEDCircles.getCirclesNo() == 0 && testEDCircles.getEllipsesNo() ==0 ){
            ROS_INFO("No circles");
            return;
        }
        if ((testEDCircles.getCirclesNo() + testEDCircles.getEllipsesNo()) == 2){
            vector<mCircle> circles = testEDCircles.getCircles();
            vector<mEllipse> ellipses = testEDCircles.getEllipses();
            Eigen::Vector3d CenterPt;
            switch(testEDCircles.getCirclesNo()){
                case TWOCIRCLES:
                    getDepth2Circles(circles);
                break;
                case ONECIRCLESONEELLIPSE:
                    getDepth1Circle1Ellipse(circles, ellipses);
                break;
                case TWOELLIPSE:
                    getDepth2Ellipses(ellipses);
                break;
            }
        }
        circle_time = (ros::Time::now()-t0).toSec();
        // ROS_INFO("Get image time: %.4f\n Remove time:%.4f\n toInt time:%.4f\n Equ time:%.4f\n Detect time:%.4f\n Draw time:%.4f\n Circle time:%.4f\n ", 
        //           get_time, remove_time, int_time, equ_time, detect_time, draw_time, circle_time);
    }
    #else
void CircleDetection::imgdepth_callback(const sensor_msgs::ImageConstPtr &img_msg){
    ros::Time t0, t1;
    double get_time, remove_time, int_time, equ_time, detect_time, draw_time, circle_time;
    t0 = ros::Time::now();
    depth_buf = getDepthImageFromMsg(img_msg);
    cv::imwrite("/home/nvidia/ibvs_nmpc_ws/src/ibvs_pkg/target/desired_d.png", depth_buf);
    get_time = (ros::Time::now()-t0).toSec();
    t0 = ros::Time::now();

    // ROS_INFO("get image");
    if (depth_buf.empty()){printf("No Depth Image"); return;}

    RemoveFarAwayPoints(depth_buf); //remove points farther away than 5m
    remove_time = (ros::Time::now()-t0).toSec();
    t0 = ros::Time::now();

    cv::Mat img_before;
    cv::Mat img;
    img_before = CircleFloat2Int(depth_buf);     //convert from float to int
    int_time = (ros::Time::now()-t0).toSec();
    t0 = ros::Time::now();

    img = EqualizeImg(img_before);     //convert from float to int
    equ_time = (ros::Time::now()-t0).toSec();
    t0 = ros::Time::now();



    // ROS_INFO("before draw");

    EDCircles testEDCircles = EDCircles(img); //detect circles and ellipses
    detect_time = (ros::Time::now()-t0).toSec();
    t0 = ros::Time::now();

    int noCircles = testEDCircles.getCirclesNo();
	// std::cout << "Number of circles: " << noCircles << std::endl;
    // ROS_INFO("circleDetected");
    cv::Mat circleImg = testEDCircles.drawResult(true, ImageStyle::BOTH);
    if (Display) {
        // cout << depth_buf.at<uint16_t>(depth_buf.rows/2, depth_buf.cols/2);
        cv::namedWindow("DepthImage", cv::WINDOW_NORMAL);
        cv::moveWindow("DepthImage", 0, 0);
        cv::resizeWindow("DepthImage", 640, 480);
        cv::imshow("DepthImage", img_before);
        
        cv::waitKey(1);

        cv::namedWindow("DepthImageAfterEqualize", cv::WINDOW_NORMAL);
        cv::moveWindow("DepthImageAfterEqualize", 0, 480);
        cv::resizeWindow("DepthImageAfterEqualize", 640, 480);
        cv::imshow("DepthImageAfterEqualize", circleImg);
        cv::waitKey(1);
    }
    draw_time = (ros::Time::now()-t0).toSec();
    t0 = ros::Time::now();
    // ROS_INFO("draw");


    

    if (testEDCircles.getCirclesNo() == 0 && testEDCircles.getEllipsesNo() ==0 ){
        ROS_INFO("No circles");
        return;
    }
    if ((testEDCircles.getCirclesNo() + testEDCircles.getEllipsesNo()) == 2){
        vector<mCircle> circles = testEDCircles.getCircles();
        vector<mEllipse> ellipses = testEDCircles.getEllipses();
        Eigen::Vector3d CenterPt;
        switch(testEDCircles.No()){
            case TWOCIRCLES:
                getDepth2Circles(circles);
            break;
            case ONECIRCLESONEELLIPSE:
                getDepth1Circle1Ellipse(circles, ellipses);
            break;
            case TWOELLIPSE:
                getDepth2Ellipses(ellipses);
            break;
        }
    }
    circle_time = (ros::Time::now()-t0).toSec();
    // ROS_INFO("Get image time: %.4f\n Remove time:%.4f\n toInt time:%.4f\n Equ time:%.4f\n Detect time:%.4f\n Draw time:%.4f\n Circle time:%.4f\n ", 
    //           get_time, remove_time, int_time, equ_time, detect_time, draw_time, circle_time);

}
#endif
        /*采用霍夫曼圆的方法识别圆圈*/
        // Eigen::Vector3d circle;
        // HoughCirclesDetect(binary_left);


    void CircleDetection::posCallback(const geometry_msgs::PoseStampedConstPtr &msg) {
        pos_ = msg;
        odom_pos_(0) = msg->pose.position.x;
        odom_pos_(1) = msg->pose.position.y;
        odom_pos_(2) = msg->pose.position.z;
        // desiredStatesPub();
        odom_orient_.w() = msg->pose.orientation.w;
        odom_orient_.x() = msg->pose.orientation.x;
        odom_orient_.y() = msg->pose.orientation.y;
        odom_orient_.z() = msg->pose.orientation.z;
    }

    void CircleDetection::RemoveFarAwayPoints(cv::Mat src, cv::Mat dst)
    {
        // depth has encoding TYPE_16UC1
        for (int row = 0; row < src.rows; row++){
            for (int col = 0; col < src.cols; col++){    
                /* 注意 Mat::at 函数是个模板函数, 需要指明参数类型, 因为这张图是具有红蓝绿三通道的图,
                所以它的参数类型可以传递一个 Vec3b, 这是一个存放 3 个 uchar 数据的 Vec(向量). 这里
                提供了索引重载, [2]表示的是返回第三个通道, 在这里是 Red 通道, 第一个通道(Blue)用[0]返回 */
                if(src.at<uint16_t>(row, col) > uint16_t(farawayTH)){
                    src.at<uint16_t>(row, col) = uint16_t(0);
                    dst.at<uchar>(row,col) = 0;
                }   
            }
        }
    }

    cv::Mat CircleDetection::DepthFloat2Int(cv::Mat src){
        cv::Mat img;
        img = cv::Mat::zeros(src.size(), CV_8UC1);

        for (int row=0; row<src.rows; ++row){
            for (int col=0; col<src.cols; col++){
                img.at<uchar>(row, col) = src.at<uint16_t>(row, col) >> 8;
                // pixel_distance = 0.001*(cv_ptr->image.at<u_int16_t>(pixel_height,pixel_width));
            }
        }
        return img; 
    }

    cv::Mat CircleDetection::ColorFloat2Int(cv::Mat src){
        cv::Mat img;
        cvtColor(src, img, COLOR_BGR2GRAY);

        // src.convertTo(img, CV_8UC1);
        // img = cv::Mat::zeros(src.size(), CV_8UC1);

        // for (int row=0; row<src.rows; ++row){
        //     for (int col=0; col<src.cols; col++){
        //         img.at<uchar>(row, col) = src.at<uchar>(row, col) >>8 2;
        //         // pixel_distance = 0.001*(cv_ptr->image.at<u_int16_t>(pixel_height,pixel_width));
        //     }
        // }
        return img; 
    }

    cv::Mat CircleDetection::filterImg(cv::Mat src){
        cv::Mat tmp, dst;
        equalizeHist(src, tmp);//equalizeHist函数对输入图像src进行直方图均衡化
        cv::Mat element = cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(7, 9));
        cv::morphologyEx(tmp, dst, cv::MORPH_OPEN, element);
        return tmp;
    }

    void CircleDetection::getDepth2Circles(const std::vector<mCircle> &circles){
        ROS_INFO("Detect 2 circles"); 
        cv::Point2d center0 = circles[0].center;
        cv::Point2d center1 = circles[1].center;
        
        double rmax = circles[0].r >= circles[1].r ? circles[0].r : circles[1].r;
        double rmin = circles[0].r < circles[1].r ? circles[0].r : circles[1].r;
        int r = 0;
        if (abs(rmax - rmin) < hypot((center0.x - center1.x), (center0.y - center1.y))) {
            ROS_INFO("Crossing circles, not ring, skip"); 
            return;
        }
        if (rmax > 2*rmin) {
            ROS_WARN("Abnormal ring size, skip");
            return;
        }

        cv::Point2d RingCenter((center0.x + center1.x)/2, (center0.y + center1.y)/2);
        if (RingCenter.x < 0 || RingCenter.x > IMAGE_WIDTH || RingCenter.y < 0 || RingCenter.y > IMAGE_HEIGHT) {
            ROS_INFO("Half of the circle outside, skip");
            return;
        }
        

        mCircle CircleOuter(RingCenter, rmax);
        mCircle CircleInside(RingCenter, rmin);
        // std::vector<Eigen::Vector3d> candidateCameraPt;
        float Centerdepth = 0;

        cv::Mat dst;
        dst = cv::Mat::zeros(depth_buf.size(), CV_8UC1);
        int idx = 0;

        for (int row=0; row<depth_buf.rows; ++row){
            for (int col=0; col<depth_buf.cols; col++){
            if (InsideCircle(CircleOuter, col, row) && OutsideCircle(CircleInside, col, row)){
                if (Display_forDebug){
                        dst.at<uint8_t>(row, col) = 255;
                }
                    Eigen::Vector3d ImagePt(col, row, depth_buf.at<uint16_t>(row, col));
                    Eigen::Vector3d CameraPt = FromImage2CameraFrame(ImagePt);
                    // candidateCameraPt.push_back(CameraPt);
                    if (CameraPt(2) >= 1000 || CameraPt(2) <= farawayTH){
                        Centerdepth = Centerdepth + CameraPt(2);
                        ++idx;
                    }
            }
            }
        }
        if (idx <= 0){
            ROS_INFO("Not enough points");
            return;
        }
        Centerdepth = Centerdepth / float(idx);
        Eigen::Vector3d CenterPt = FromImage2CameraFrame(Eigen::Vector3d(RingCenter.x, RingCenter.y, Centerdepth));
        ROS_INFO_STREAM("current ring center:" << RingCenter.x  << ' ' << RingCenter.y << ' ' << Centerdepth);
        ROS_INFO_STREAM("current ring center in image is:" << CenterPt(0) << ' ' << CenterPt(1)<< ' ' << CenterPt(2));
        if (Display_forDebug) {
            cv::namedWindow("Circle", cv::WINDOW_NORMAL);
            cv::moveWindow("Circle", 1920, 0);
            cv::resizeWindow("Circle", 640, 480);
            cv::imshow("Circle", dst);
            cv::waitKey(1); 
        }
        PublishResult(CenterPt);
    }

    //-------------------if 1 circle and 1 ellipse-------------
    void CircleDetection::getDepth1Circle1Ellipse(const std::vector<mCircle> &circles, const std::vector<mEllipse> &ellipses){
        ROS_INFO("Detect 1 circle and 1 ellipse"); 
        cv::Point2d center0 = circles[0].center;
        cv::Point2d center1 = ellipses[0].center;

        int r = 0;
        if (abs(circles[0].r - ellipses[0].axes.width) < hypot((center0.x - center1.x), (center0.y - center1.y)) &&  abs(circles[0].r -ellipses[0].axes.height) < hypot((center0.x - center1.x), (center0.y - center1.y))) {
            ROS_INFO("Crossing circle and ellipse, not ring, skip");
            return;
        }

        cv::Point2d RingCenter((center0.x + center1.x)/2, (center0.y + center1.y)/2);
        if (RingCenter.x < 0 || RingCenter.x > IMAGE_WIDTH || RingCenter.y < 0 || RingCenter.y > IMAGE_HEIGHT) {
            ROS_INFO("Half of the circle outside, skip");
            return;
        }
        
        // std::vector<Eigen::Vector3d> candidateCameraPt;
        float Centerdepth = 0;

        cv::Mat dst;
        dst = cv::Mat::zeros(depth_buf.size(), CV_8UC1);
        int idx = 0;

        for (int row=0; row<depth_buf.rows; ++row){
            for (int col=0; col<depth_buf.cols; col++){
                if((InsideEllipse(ellipses[0], col, row) && !InsideCircle(circles[0], col, row)) || (InsideCircle(circles[0], col, row) && !InsideEllipse(ellipses[0], col, row))){
                if (Display_forDebug){
                        dst.at<uint8_t>(row, col) = 255;
                }
                    Eigen::Vector3d ImagePt(col, row, depth_buf.at<uint16_t>(row, col));
                    Eigen::Vector3d CameraPt = FromImage2CameraFrame(ImagePt);
                    // candidateCameraPt.push_back(CameraPt);
                    if (CameraPt(2) >= 1000 || CameraPt(2) <= farawayTH){
                        Centerdepth = Centerdepth + CameraPt(2);
                        ++idx;
                    }
            }
            }
        }
        if (idx <= 0){
            ROS_INFO("Not enough points");
            return;
        }
        Centerdepth = Centerdepth / float(idx);
        Eigen::Vector3d CenterPt = FromImage2CameraFrame(Eigen::Vector3d(RingCenter.x, RingCenter.y, Centerdepth));
        ROS_INFO_STREAM("current ring center:" << RingCenter.x  << ' ' << RingCenter.y << ' ' << Centerdepth);
        ROS_INFO_STREAM("current image center in image is:" << CenterPt(0) << ' ' << CenterPt(1)<< ' ' << CenterPt(2));
        if (Display_forDebug) {
            cv::namedWindow("1e1c", cv::WINDOW_NORMAL);
            cv::moveWindow("1e1c", 1920, 0);
            cv::resizeWindow("1e1c", 640, 480);
            cv::imshow("1e1c", dst);
            cv::waitKey(1); 
        }
        PublishResult(CenterPt);
    }

    //-------------------if 2 ellipses----------------
    void CircleDetection::getDepth2Ellipses(const std::vector<mEllipse> &ellipses){
        ROS_INFO("Detect 2 ellipses"); 
        cv::Point2d center0 = ellipses[0].center;
        cv::Point2d center1 = ellipses[1].center;

        int r = 0;
        //may cause problem
        if (abs(ellipses[1].axes.height - ellipses[0].axes.height) < hypot((center0.x - center1.x), (center0.y - center1.y))) {
            ROS_INFO("Crossing circle and ellipse, not ring, skip");
            return;
        }

        cv::Point2d RingCenter((center0.x + center1.x)/2, (center0.y + center1.y)/2);
        if (RingCenter.x < 0 || RingCenter.x > IMAGE_WIDTH || RingCenter.y < 0 || RingCenter.y > IMAGE_HEIGHT) {
            ROS_INFO("Half of the ellipse outside, skip");
            return;
        }
        
        // std::vector<Eigen::Vector3d> candidateCameraPt;
        float Centerdepth = 0;

        cv::Mat dst;
        dst = cv::Mat::zeros(depth_buf.size(), CV_8UC1);
        int idx = 0;

        for (int row=0; row<depth_buf.rows; ++row){
            for (int col=0; col<depth_buf.cols; col++){
                if((InsideEllipse(ellipses[0], col, row) && !InsideEllipse(ellipses[1], col, row)) || (InsideEllipse(ellipses[0], col, row) && !InsideEllipse(ellipses[1], col, row))){
                if (Display_forDebug){
                        dst.at<uint8_t>(row, col) = 255;
                }
                    Eigen::Vector3d ImagePt(col, row, depth_buf.at<uint16_t>(row, col));
                    Eigen::Vector3d CameraPt = FromImage2CameraFrame(ImagePt);
                    // candidateCameraPt.push_back(CameraPt);
                    if (CameraPt(2) >= 1000 || CameraPt(2) <= farawayTH){
                        Centerdepth = Centerdepth + CameraPt(2);
                        ++idx;
                    }
            }
            }
        }
        if (idx <= 0){
            ROS_INFO("Not enough points");
            return;
        }
        Centerdepth = Centerdepth / float(idx);
        Eigen::Vector3d CenterPt = FromImage2CameraFrame(Eigen::Vector3d(RingCenter.x, RingCenter.y, Centerdepth));
        ROS_INFO_STREAM("current ring center:" << RingCenter.x  << ' ' << RingCenter.y << ' ' << Centerdepth);
        ROS_INFO_STREAM("current image center in image is:" << CenterPt(0) << ' ' << CenterPt(1)<< ' ' << CenterPt(2));


        if (Display_forDebug) {
            cv::namedWindow("2e", cv::WINDOW_NORMAL);
            cv::moveWindow("2e", 1920, 0);
            cv::resizeWindow("2e", 640, 480);
            cv::imshow("2e", dst);
            cv::waitKey(1); 
        }
        PublishResult(CenterPt);
    }


    bool CircleDetection::InsideCircle(const mCircle &circle, float ptx, float pty){
        // cout << circle.center.x << ' ' << circle.center.y<< ' ' << circle.r<< ' ' << pt[0] << ' '<< pt[1];
        return (ptx-circle.center.x)*(ptx-circle.center.x) + (pty-circle.center.y) * (pty-circle.center.y) <= circle.r * circle.r;
    }

    bool CircleDetection::OutsideCircle(const mCircle &circle, float ptx, float pty){
        // cout << circle.center.x << ' ' << circle.center.y<< ' ' << circle.r<< ' ' << pt[0] << ' '<< pt[1];
        return (ptx-circle.center.x)*(ptx-circle.center.x) + (pty-circle.center.y) * (pty-circle.center.y) >= circle.r * circle.r;
    }

    bool CircleDetection::InsideEllipse(const mEllipse &ellipse, float pt_x, float pt_y){
        // cout << ellipse.center.x << ' ' << ellipse.center.y<< ' ' << ellipse.theta<< ' ' << ellipse.axes.height << ' '<< ellipse.axes.width;
        return ((pt_x-ellipse.center.x)*cos(ellipse.theta)+(pt_y-ellipse.center.y)*sin(ellipse.theta))*((pt_x-ellipse.center.x)*cos(ellipse.theta)+(pt_y-ellipse.center.y)*sin(ellipse.theta))/(double)(ellipse.axes.width * ellipse.axes.width)
            + ((pt_y-ellipse.center.y)*cos(ellipse.theta)-(pt_x-ellipse.center.x)*sin(ellipse.theta))*((pt_y-ellipse.center.y)*cos(ellipse.theta)-(pt_x-ellipse.center.x)*sin(ellipse.theta))/(double)(ellipse.axes.height * ellipse.axes.height)
            <= 1.0;
    }


    Eigen::Vector3d CircleDetection::FromImage2CameraFrame(const Eigen::Vector3d &v1){
        Vector3d v(v1(0), v1(1), 1);
        return v1(2) * camera_K_inv_ * v;
    }

    // void CircleDetection::transformCamera2World(const Eigen::Vector3d &point_b) {
    //     // Eigen::Matrix3d R_c_b;
    //     // R_c_b << 0, 0, 1, 1, 0, 0, 0, 1, 0;
    //     // Eigen::Vector3d t_c_b (0.26, 0, 0);
    //     Eigen::Vector3d point_b;
    //     point_b(0) = double(CenterPt(2)/1000.0);
    //     point_b(1) = double(-1.0*CenterPt(0)/1000.0);
    //     point_b(2) = double(-1.0*CenterPt(1)/1000.0) - 0.070;
    //     Eigen::Matrix3d R_b_w = odom_orient_.normalized().toRotationMatrix();
    //     Eigen::Vector3d t_b_w = odom_pos_;
    //     Eigen::Vector3d point_w = R_b_w * point_b + t_b_w;
    //     PublishResult(point_w);
    // }

    void CircleDetection::PublishResult(const Eigen::Vector3d &CenterPt){
        custom_msgs::CircleRecog recg;
        recg.header.stamp = ros::Time::now();
        recg.header.frame_id = "world";

        //turn from camera frame to body frame, under camera frame, the unit is mm, under body the unit is m
        // recg.point.x = double(point_w(2)/1000.0);
        // recg.point.y = double(-1.0*point_w(0)/1000.0); 
        // recg.point.z = double(-1.0*point_w(1)/1000.0) - 0.070;
        recg.point.x = double(CenterPt(2)/1000.0);
        recg.point.y = double(-1.0*CenterPt(0)/1000); 
        // recg.point.z = double(-1.0*CenterPt(1)/1000.0) - 0.070;
        recg.point.z = double(-1.0*CenterPt(1)/1000.0) - 0.070;
        // recg.point.x = point_w(0);
        // recg.point.y = point_w(1);
        // recg.point.z = point_w(2);
        recg.vector.x = -1;
        recg.vector.y = 0;
        recg.vector.z = 0;

        //from body frame to world frame
        Eigen::Vector3d point_b;
        point_b(0) = recg.point.x;
        point_b(1) = recg.point.y;
        point_b(2) = recg.point.z;
        Eigen::Matrix3d R_b_w = odom_orient_.normalized().toRotationMatrix();
        Eigen::Vector3d t_b_w = odom_pos_;
        Eigen::Vector3d point_w = R_b_w * point_b + t_b_w;
        recg.point.x = point_w(0); 
        recg.point.y = point_w(1)+1.2;
        recg.point.z = point_w(2)+1.36;
        pub_recg.publish(recg);
    }

    cv::Mat CircleDetection::computesgbm(const sensor_msgs::ImageConstPtr &left_image,const sensor_msgs::ImageConstPtr &right_image)
    /*SGBM_matching SGBM算法
    *@param Mat &left_image :左图像
    *@param Mat &right_image:右图像
    */
    {
        cv::Mat L = cv_bridge::toCvShare(left_image)->image;
        cv::Mat R = cv_bridge::toCvShare(right_image)->image;
        enum mode_view { ZUO, YOU };
 	    mode_view view;	//输出左视差图or右视差图
        Mat disp;
    
        int numberOfDisparities = ((L.size().width / 8) + 15)&-16;
        Ptr<StereoSGBM> sgbm = StereoSGBM::create(0, 16, 3);
        sgbm->setPreFilterCap(32);
    
        int SADWindowSize = 5;
        int sgbmWinSize = SADWindowSize > 0 ? SADWindowSize : 3;
        sgbm->setBlockSize(sgbmWinSize);//设置了预过滤容量为32，用于控制立体匹配算法中预过滤器的参数，影响匹配结果的质量和计算速度
        int cn = L.channels();
    
        sgbm->setP1(8 * cn*sgbmWinSize*sgbmWinSize);
        sgbm->setP2(32 * cn*sgbmWinSize*sgbmWinSize);
        sgbm->setMinDisparity(0);
        sgbm->setNumDisparities(numberOfDisparities);
        sgbm->setUniquenessRatio(10);
        sgbm->setSpeckleWindowSize(100);
        sgbm->setSpeckleRange(32);
        sgbm->setDisp12MaxDiff(1);
    
    
        Mat left_gray, right_gray;
        cvtColor(L, left_gray, CV_BGR2GRAY);
        cvtColor(R, right_gray, CV_BGR2GRAY);
        // // 将图像分为4块进行计算
        // cv::parallel_for_(cv::Range(0, left_gray.rows/4), computeDisparityThread, left_gray, right_gray, disp); 
        // cv::parallel_for_(cv::Range(left_gray.rows/4, left_gray.rows/2), computeDisparityThread, left_gray, right_gray, disp);
        // cv::parallel_for_(cv::Range(left_gray.rows/2, left_gray.rows*3/4), computeDisparityThread, left_gray, right_gray, disp);
        // cv::parallel_for_(cv::Range(left_gray.rows*3/4, left_gray.rows), computeDisparityThread, left_gray, right_gray, disp);
        // //...重复调用计算其他两块
        view = ZUO;
        if (view == ZUO)	//计算左视差图
        {
            sgbm->compute(left_gray, right_gray, disp);
    
            disp.convertTo(disp, CV_32F, 1.0 / 16);			//除以16得到真实视差值
    
            Mat disp8U = Mat(disp.rows, disp.cols, CV_8UC1);
            normalize(disp, disp8U, 0, 255, NORM_MINMAX, CV_8UC1);
            //imwrite("results/SGBM.jpg", disp8U);
    
            return disp8U;
        }
        else if (view == YOU)	//计算右视差图
        {
            sgbm->setMinDisparity(-numberOfDisparities);
            sgbm->setNumDisparities(numberOfDisparities);
            sgbm->compute(left_gray, right_gray, disp);
    
            disp.convertTo(disp, CV_32F, 1.0 / 16);			//除以16得到真实视差值
    
            Mat disp8U = Mat(disp.rows, disp.cols, CV_8UC1);
            normalize(disp, disp8U, 0, 255, NORM_MINMAX, CV_8UC1);
            //imwrite("results/SGBM.jpg", disp8U);
    
            return disp8U;
        }
        else
        {
            return Mat();
        }
    }

    // cv::Mat CircleDetection::getDepthImageFromSence(const cv::Mat &disparity)//将视差图传进去，计算深度图
    // {
    //     cv::Mat img = disparity;
    //     //cv::Mat depth(480,640,CV_16S);
    //     cv::Mat depth(480,640,CV_16S);
    //     for (int col = 0; col <640; col++)
    //     {
    //         for (int row = 0; row <480; row++)
    //         {
    //             short d = img.ptr<uchar>(row)[col];
    //             if (d == 0) continue;
    //             depth.ptr<short>(row)[col] = fx * baseline / d;
    //         }
    //     }
    //     // depth_ptr = cv_bridge::toCvCopy(left_image, sensor_msgs::image_encodings::TYPE_16UC1);
    //     return depth;
    // }
        cv::Mat CircleDetection::getDepthImageFromSence(const cv::Mat &disparity)//将视差图传进去，计算深度图
    {
        // cv::Mat img = disparity;
        // cv::Mat depth(480,640,CV_16S);
        // for (int col = 0; col <640; col++)
        // {
        //     for (int row = 0; row <480; row++)
        //     {
        //         short d = img.ptr<uchar>(row)[col];
        //         if (d == 0) continue;
        //         depth.ptr<short>(row)[col] = fx * baseline / d;
        //     }
        // }
        // // depth_ptr = cv_bridge::toCvCopy(left_image, sensor_msgs::image_encodings::TYPE_16UC1);
        // return depth;

        cv::Mat depth(disparity.rows,disparity.cols,CV_16UC1);
// 		for(int i = 0; i < disparity_im.rows; i++) {
// 			for(int j = 0; j < disparity_im.cols; j++) {
// 				const float d = disparity_im.at<float>(i, j)*256.0f;
// 				disparity16.at<uint16_t>(i, j) = (uint16_t) d;
        for (int row = 0; row < disparity.rows; row++)
        {
            for (int col = 0; col < disparity.cols; col++)
            {
                uint8_t d = disparity.at<uint8_t>(row,col);
                if (d < 2) {
                    depth.ptr<uint16_t>(row)[col] = 20;
                }
                else{
                    depth.ptr<uint16_t>(row)[col] = uint16_t( fx * baseline / d);
                }
				// const float d = disparity.at<float>(row, col)*256.0f;
				// disparity8.at<uint8_t>(row, col) = (uint8_t) (d /10.0f * 256.0f);
				// if ( col > 400 && col < 500)
				    // cout << "dp:" << depth.ptr<float>(row)[col] << endl;
            }
        }
        // depth_ptr = cv_bridge::toCvCopy(left_image, sensor_msgs::image_encodings::TYPE_16UC1);
        // 创建一个窗口并显示图像
		// cv::imshow("depth", disparity8);

		// 等待按下任意键
		// cv::waitKey(0);
		return depth;
    }
};

// int main(int argc,char *argv[])
// {
//     ros::init(argc,argv,"imagedetect");
//     circle_detection node;
//     // ros::Rate loop_rate(5);
//     //ros::spinOnce();

//     return 0;
// }
